Distance Function

FUNCTION Distance &
!
(point1, point2)

USE Units, ONLY: &
!Imported parameters:
degToRad

IMPLICIT NONE

!Arguments with intent in:
TYPE (Coordinate) , INTENT (IN):: point1,point2

!Local declarations:
REAL (KIND = double):: distance
REAL (KIND = float) :: maxnri,iconv,iter
TYPE (Ellipsoid)    :: a,b,inv_f
REAL (KIND = float) :: U1,U2,ell_a,ell_b,ell_f
REAL (KIND = float) :: LL, senSIGMA, cosSIGMA, SIGMA, senALFA
REAL (KIND = float) :: COS2ALFA,COS2SIGMAm,CC,LP,U2U,AA,BB,deltaSIGMA,LAMBDA
!----------------------end of declarations-------------------------------------

iconv=0
maxnri=100
iter=1

ell_a = point1 % system % ellipsoid % a
ell_b = point1 % system % ellipsoid % b
ell_f = 1./point1 % system % ellipsoid % inv_f 
   
SELECT CASE (point1 % system  % system)
  CASE (GEODETIC)
!    !Vincenty formula (NON FUNZIONA ALL'EQUATORE)
!    ell_a = punto1 % system % ellipsoid % a
!    ell_b = punto1 % system % ellipsoid % b
!    ell_f = 1/punto1 % system % ellipsoid % inv_f 
!    U1= atan((1-ell_f)*tan(punto1 % northing* degToRad))
!    U2= atan((1-ell_f)*tan(punto2 % northing* degToRad))
!    LAMBDA=(punto2 % easting - punto1 % easting)* degToRad !conversion to radians
!    LL=LAMBDA
!    DO WHILE(iconv==0 .AND. iter<maxnri)
!     senSIGMA=SQRT((cos(U2)*SIN(LL))**2+(COS(U1)*SIN(U2)-SIN(U1)*COS(U2)*COS(LL))**2)
!     IF(senSIGMA==0) THEN
!        write (*,*)'punti coincidenti'
!        pause
!     END IF
!     cosSIGMA=SIN(U1)*SIN(U2)+COS(U1)*COS(U2)*COS(LL)
!     SIGMA=ATAN2(senSIGMA,cosSIGMA)!SIGMA=ATAN(senSIGMA/cosSIGMA)
!     senALFA=COS(U1)*COS(U2)*SIN(LL)/senSIGMA
!     COS2ALFA=1-senALFA**2
!     COS2SIGMAm=cosSIGMA-2*SIN(U1)*SIN(U2)/COS2ALFA
!     CC=ell_f/16*COS2ALFA*(4+ell_f*(4-3*COS2ALFA))
!     LP=LL
!     LL=LAMBDA+(1-CC)*ell_f*senALFA*(SIGMA+CC*senSIGMA* &
!        (COS2SIGMAm+CC*cosSIGMA*(-1+2*COS2SIGMAm*COS2SIGMAm)))
!     IF(abs(LL-LP)<10.**(-12))THEN 
!	   iconv=1 
!     END IF
!     iter=iter+1
!    END DO
!    
!    U2U=COS2ALFA*(ell_a**2 - ell_b**2)/(ell_b**2)
!    AA=1+U2U/16384.*(4096+U2U*(-768+U2U*(320-175*U2U)))
!    BB=U2U/1024*(256+U2U*(-128+U2U*(74-47*U2U)))
!    deltaSIGMA=BB*senSIGMA*(COS2SIGMAm+BB/4.* &
!               (cosSIGMA*(1-2*COS2SIGMAm*COS2SIGMAm)-BB/6.*COS2SIGMAm* &
!               (-3+4*senSIGMA*senSIGMA)*(-3+4*COS2SIGMAm*COS2SIGMAm)))
!    distance=ell_b * AA * (SIGMA-deltaSIGMA)
!    WRITE(*,*)DISTANCE
    
    
    ! great circle distance formula (approximate solution)
    IF ((SIN(point1 % northing* degToRad) * SIN(point2 % northing* degToRad)+ &
         COS(point1 % northing* degToRad) * COS(point2 % northing* degToRad) * &
         COS(point1 % easting* degToRad - point2 % easting* degToRad)) < 1.) THEN         
         distance = 6378.7* 1000. * ACOS(SIN(point1 % northing* degToRad) * &
                    SIN(point2 % northing* degToRad)+ &
                   (COS(point1 % northing* degToRad) * COS(point2 % northing* degToRad) * &
                   COS(point2 % easting* degToRad - point1 % easting* degToRad)))
             
    ELSE
      distance=6378.7* 1000. *ACOS(0.9999999)
    END IF 
    
    
  CASE DEFAULT !projected coordinate reference system
       
    distance = SQRT((point1 % northing - point2 % northing)**2 + &
             (point1 % easting - point2 % easting)**2)
    
  END SELECT 

END  FUNCTION Distance